/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-02-10     RT-Thread    first version
 */

#include <stdio.h>
#include <board.h>
#include "at32f4xx.h"

#include "tarm_tim.h"
#include "tarm_exit.h"
#include "tarm_math.h"
#include "tarm_uart.h"
#include "tarm_iic.h"

#include "hardware/tarm_io.h"
#include "hardware/tarm_pwm.h"
#include "hardware/tarm_limit.h"
#include "hardware/tarm_baseTimer.h"
#include "hardware/tarm_communication.h"
#include "hardware/closeloop/closeloop.h"

#include "application/tarm_run.h"
#include "application/tarm_init.h"
#include "application/tarm_command.h"

void MainInit(void);
void ReportStruct_Handle(void);

int main(void)
{
    uint16_t t_counter=0;
    AT32_LEDn_Init();
    AT32_LEDn_OFF(LED2);
    AT32_LEDn_OFF(LED3);
    AT32_LEDn_OFF(LED4);

    USART_Cmd(USART1, DISABLE);
    MainInit();
    SYSLED_RESET;

//    PRINTF_UART0("\r\n1\r\n");
//    CloseLoop_Init();
//    PRINTF_UART0("\r\n2\r\n");
//    PRINTF_UART1("\r\n2\r\n");
//    SteeringIO_Init();
//    uint8_t res;
//    I2C1_Init();
//    MPU_Get_Temperature();
//    uint8_t i_dateAddr[4];
//    IIC_readArray(0x68, 0x75-3, i_dateAddr, 4);
//    MPU_ALL_Update();
//    MPU_Get_Temperature();
//    I2C_Master_Receive(I2C_PORT, 0x68<<1, i_dateAddr, 2, 1000);
//    I2C_Master_Rec(I2C_PORT, 0x68<<1, i_dateAddr, 2, 1000);
//    IIC_readArray(0x68, 0x41, &res, 1);
//    MPU_Init();

//    PwmTimer_Init();
//    uint32_t value=100;

    while (1)
    {
//        uart_tips();
        uart2_tips();

        ReportStruct_Handle();

        if(t_counter==100) SYSLED_SET;
        else if(t_counter==200) SYSLED_RESET;
        t_counter=t_counter%200;
        t_counter++;
    }

    return 0;
}


void MainInit(void)
{
    //
    SteeringIO_Init();
//    LimitIO_Init();

//    Uart1DMA_Configuration();
    PRINTF_UART0("\r\nUART is initial, welcome to TARM V1.1\r\n");
    Uart2DMA_Configuration();
    PRINTF_UART1("\r\nUART is initial, welcome to TARM V1.1\r\n");

    TimeBase_InitUnstart();
    TimePV_InitUnstart(1000);
    PRINTF_UART0("\r\ntime base/pv enable\r\n");
    PRINTF_UART1("\r\ntime base/pv disable\r\n");
    //
    I2C1_Init();
    if(CloseLoop_Init()!=1){
        CloseLoopSingle.CloseLoopFlag = Open_CloseLoop;
        PRINTF_UART0("OPENLOOP FINISH\r\n");
        PRINTF_UART1("OPENLOOP FINISH\r\n");
    }
    else{
        PRINTF_UART0("CLOOPLOOP FINISH\r\n");
        PRINTF_UART1("CLOOPLOOP FINISH\r\n");
    }

//*
    //
    LimitIO_Init();
    PRINTF_UART0("LimitIO_Init\r\n");
    PRINTF_UART1("LimitIO_Init\r\n");
    io_init();
    PRINTF_UART0("io_init\r\n");
    PRINTF_UART1("io_init\r\n");
    locate_init();
    PRINTF_UART0("locate_init\r\n");
    PRINTF_UART1("locate_init\r\n");

    //
    CloseLoopUpdate();
    PRINTF_UART0("OPEN/CLOOPLOOP update\r\n");
    PRINTF_UART1("OPEN/CLOOPLOOP update\r\n");

//    if(CloseLoopSingle.CloseLoopFlag == Open_CloseLoop)
//        LimitExit_Init();
//  PRINTF("\r\nlimit init \r\n");

    PwmTimer_Init();
    PRINTF_UART0("\r\n PWM enable \r\n");

    switch(CloseLoopSingle.CloseLoopFlag)
    {
    case Open_CloseLoop:
        PRINTF_UART0("\r\n Open Loop \r\n");
        break;
    case ADXL_CloseLoop:
        PRINTF_UART0("\r\n ADXL CloseLoop \r\n");
        break;
    case MPU_CloseLoop:
        PRINTF_UART0("\r\n MPU CloseLoop \r\n");
        break;
    }
//*/
#ifdef _DEBUG_TARM_SPEED_DELAY_TIME_
    SpeedDebugTimer_init();
#endif
}

void ReportStruct_Handle(void)
{
//  static u16 color = GREEN;
    char ReportStr[1500]="->";
    char ReportStr_Assist[200]="0";

    //1.1、处理UART1
    if(ReportStruct.Uart1ReportFlag.ExecuteFinish == 1){
        sprintf(ReportStr_Assist,"ExecuteFinish;\n");
        strcat(ReportStr, ReportStr_Assist);
        ReportStruct.Uart1ReportFlag.ExecuteFinish = 0;

        if(ReportStruct.Uart1ReportFlag.CloseLoopReport == 1){
            sprintf(ReportStr_Assist,"CloseLoop Result:\n");
            strcat(ReportStr, ReportStr_Assist);
            ReportStruct.Uart1ReportFlag.CloseLoopReport = 0;
        }
        else{
            sprintf(ReportStr_Assist,"OpenLoop Result:\n");
            strcat(ReportStr, ReportStr_Assist);
        }

        sprintf(ReportStr_Assist,"\t_&x:");
        strcat(ReportStr, ReportStr_Assist);
        memset(ReportStr_Assist, 0, sizeof(ReportStr_Assist));
        ftoa(ReportStr_Assist, g_TARM_RunRecord.pv_current.x);
        strcat(ReportStr, ReportStr_Assist);

        sprintf(ReportStr_Assist,";\t_&y:");
        strcat(ReportStr, ReportStr_Assist);
        memset(ReportStr_Assist, 0, sizeof(ReportStr_Assist));
        ftoa(ReportStr_Assist, g_TARM_RunRecord.pv_current.y);
        strcat(ReportStr, ReportStr_Assist);

        sprintf(ReportStr_Assist,";\t_&prime:");
        strcat(ReportStr, ReportStr_Assist);
        memset(ReportStr_Assist, 0, sizeof(ReportStr_Assist));
        ftoa(ReportStr_Assist, g_TARM_RunRecord.pv_current.p);
        strcat(ReportStr, ReportStr_Assist);

        sprintf(ReportStr_Assist,";\t_&vice:");
        strcat(ReportStr, ReportStr_Assist);
        memset(ReportStr_Assist, 0, sizeof(ReportStr_Assist));
        ftoa(ReportStr_Assist, g_TARM_RunRecord.pv_current.v);
        strcat(ReportStr, ReportStr_Assist);

        sprintf(ReportStr_Assist,";\r\r\n");
        strcat(ReportStr, ReportStr_Assist);

    }
    else if(ReportStruct.Uart1ReportFlag.VerifySuccess == 1){
        sprintf(ReportStr_Assist,"VerifySuccess;\n");
        strcat(ReportStr, ReportStr_Assist);
        ReportStruct.Uart1ReportFlag.VerifySuccess = 0;
    }
    else if(ReportStruct.Uart1ReportFlag.VerifyError == 1){
        sprintf(ReportStr_Assist,"VerifyError;\n");
        strcat(ReportStr, ReportStr_Assist);
        ReportStruct.Uart1ReportFlag.VerifyError = 0;
    }


    //1.2、
    if(ReportStruct.Uart1ReportFlag.CloseLoop_ErrorFlag == 1){
        sprintf(ReportStr_Assist,"LoopError;\n");
        strcat(ReportStr, ReportStr_Assist);

        PRINTF_UART0("loop error\n");
        ReportStruct.Uart1ReportFlag.CloseLoop_ErrorFlag = 0;
    }

    //1.3、上报并清空字符串
    if(strlen(ReportStr)>2)
        PRINTF_UART0(ReportStr);
    memset(ReportStr, 0, sizeof(ReportStr));
    strcpy(ReportStr, "->");


    //2.1、处理串口2
    if(ReportStruct.Uart2ReportFlag.ExecuteFinish == 1){
        sprintf(ReportStr_Assist,"ExecuteFinish;\r\n");
        strcat(ReportStr, ReportStr_Assist);
        ReportStruct.Uart2ReportFlag.ExecuteFinish = 0;

        if(ReportStruct.Uart2ReportFlag.CloseLoopReport == 1){
            sprintf(ReportStr_Assist,"CloseLoop Result:\r\n");
            strcat(ReportStr, ReportStr_Assist);
            ReportStruct.Uart2ReportFlag.CloseLoopReport = 0;
        }
        else{
            sprintf(ReportStr_Assist,"OpenLoop Result:\r\n");
            strcat(ReportStr, ReportStr_Assist);
        }

        sprintf(ReportStr_Assist,"\t_&x:");
        strcat(ReportStr, ReportStr_Assist);
        memset(ReportStr_Assist, 0, sizeof(ReportStr_Assist));
        ftoa(ReportStr_Assist, g_TARM_RunRecord.pv_current.x);
        strcat(ReportStr, ReportStr_Assist);

        sprintf(ReportStr_Assist,";\t_&y:");
        strcat(ReportStr, ReportStr_Assist);
        memset(ReportStr_Assist, 0, sizeof(ReportStr_Assist));
        ftoa(ReportStr_Assist, g_TARM_RunRecord.pv_current.y);
        strcat(ReportStr, ReportStr_Assist);

        sprintf(ReportStr_Assist,";\t_&prime:");
        strcat(ReportStr, ReportStr_Assist);
        memset(ReportStr_Assist, 0, sizeof(ReportStr_Assist));
        ftoa(ReportStr_Assist, g_TARM_RunRecord.pv_current.p);
        strcat(ReportStr, ReportStr_Assist);

        sprintf(ReportStr_Assist,";\t_&vice:");
        strcat(ReportStr, ReportStr_Assist);
        memset(ReportStr_Assist, 0, sizeof(ReportStr_Assist));
        ftoa(ReportStr_Assist, g_TARM_RunRecord.pv_current.v);
        strcat(ReportStr, ReportStr_Assist);

        sprintf(ReportStr_Assist,";\r\r\n");
        strcat(ReportStr, ReportStr_Assist);

    }
    else if(ReportStruct.Uart2ReportFlag.VerifySuccess == 1){
        sprintf(ReportStr_Assist,"VerifySuccess;\r\n");
        strcat(ReportStr, ReportStr_Assist);
        ReportStruct.Uart2ReportFlag.VerifySuccess = 0;
    }
    else if(ReportStruct.Uart2ReportFlag.VerifyError == 1){
        sprintf(ReportStr_Assist,"VerifyError;\r\n");
        strcat(ReportStr, ReportStr_Assist);
        ReportStruct.Uart2ReportFlag.VerifyError = 0;
    }


    //2.2、
    if(ReportStruct.Uart2ReportFlag.CloseLoop_ErrorFlag == 1){
        sprintf(ReportStr_Assist,"LoopError;\r\n");
        strcat(ReportStr, ReportStr_Assist);

//      if(color==GREEN)
//          color = YELLOW;
//      else
//          color = GREEN;
        PRINTF_UART0("loop error\r\n");
//      Gui_DrawFont_GBK16(20,300,RED,color,(u8 *)"err:loop");
        ReportStruct.Uart2ReportFlag.CloseLoop_ErrorFlag = 0;
    }

    //2.3、上报\清空
//  if(strlen(ReportStr)>2)
//      esp8266_report_command(ReportStr,strlen(ReportStr));
    if(strlen(ReportStr)>2)
        PRINTF_UART1(ReportStr);
    memset(ReportStr, 0, sizeof(ReportStr));
    memset(ReportStr_Assist, 0, sizeof(ReportStr_Assist));

    //3.1、处理debug数据
#ifdef _DEBUG_TARM_SPEED_DELAY_TIME_
    if(ReportStruct.SpeedDebugStruct.ReportFlag == 1)
    {
        sprintf(ReportStr_Assist,"min speed ForTheory:%dus, min time for realy:%dus\r\n",
                    ReportStruct.SpeedDebugStruct.MinTimeForTheory,
                    ReportStruct.SpeedDebugStruct. MinTimeForReally);
        strcat(ReportStr, ReportStr_Assist);
        sprintf(ReportStr_Assist,"the running debug:\r\n");
        strcat(ReportStr, ReportStr_Assist);
        strcat(ReportStr, ReportStruct.SpeedDebugStruct.Buffer);
        PRINTF_UART0(ReportStr);
        PRINTF_UART0("\r\n");
        //reset
        ReportStruct.SpeedDebugStruct.MinTimeForReally=0;
        ReportStruct.SpeedDebugStruct.MinTimeForTheory=0;
        ReportStruct.SpeedDebugStruct.ReportFlag=2;
        memset(ReportStruct.SpeedDebugStruct.Buffer, 0, strlen(ReportStruct.SpeedDebugStruct.Buffer));
    }
#endif
}


